/*
 * RT-Thread Secure
 *
 * Copyright (c) 2021, Shanghai Real-Thread Electronic Technology Co., Ltd.
 *
 * All rights reserved.
 */
#include <rtthread.h>
#include <rtdevice.h>
#include <board.h>
#include "rk3568_clk.h"
#include "rk3568.h"

#define PLL_CON(ch, con) ((ch)*0x20+((con)*4))
#define PMU_PLL_CON(ch, con) ((ch)*0x40+((con)*4))

#define RK3568_PLL_RATE(_rate, _refdiv, _fbdiv, \
    _postdiv1, _postdiv2, _dsmpd, _frac)        \
{                                               \
    .rate     = _rate##U,                       \
    .fbdiv    = _fbdiv,                         \
    .postdiv1 = _postdiv1,                      \
    .refdiv   = _refdiv,                        \
    .postdiv2 = _postdiv2,                      \
    .dsmpd    = _dsmpd,                         \
    .frac     = _frac,                          \
}

struct rockchip_pll_cfg
{
    rt_uint64_t rate;
    rt_uint32_t fbdiv;
    rt_uint32_t postdiv1;
    rt_uint32_t refdiv;
    rt_uint32_t postdiv2;
    rt_uint32_t dsmpd;
    rt_uint32_t frac;
};
typedef struct rockchip_pll_cfg rockchip_pll_cfg_t;

struct rk3568_pll_pdata
{
    rt_ubase_t cru;
    rt_uint8_t ch;
};
typedef struct rk3568_pll_pdata rk3568_pll_pdata_t;

/* 提前设定好的 PLL 配置, 加速配置, 前提是 24 MHz 晶振 */
static rockchip_pll_cfg_t cfgs[] = {
    /* mhz, refdiv, fbdiv, postdiv1, postdiv2, dsmpd, frac */
	RK3568_PLL_RATE(2208000000, 1, 92, 1, 1, 1, 0),
	RK3568_PLL_RATE(2184000000, 1, 91, 1, 1, 1, 0),
	RK3568_PLL_RATE(2160000000, 1, 90, 1, 1, 1, 0),
	RK3568_PLL_RATE(2088000000, 1, 87, 1, 1, 1, 0),
	RK3568_PLL_RATE(2064000000, 1, 86, 1, 1, 1, 0),
	RK3568_PLL_RATE(2040000000, 1, 85, 1, 1, 1, 0),
	RK3568_PLL_RATE(2016000000, 1, 84, 1, 1, 1, 0),
	RK3568_PLL_RATE(1992000000, 1, 83, 1, 1, 1, 0),
	RK3568_PLL_RATE(1920000000, 1, 80, 1, 1, 1, 0),
	RK3568_PLL_RATE(1896000000, 1, 79, 1, 1, 1, 0),
	RK3568_PLL_RATE(1800000000, 1, 75, 1, 1, 1, 0),
	RK3568_PLL_RATE(1704000000, 1, 71, 1, 1, 1, 0),
	RK3568_PLL_RATE(1608000000, 1, 67, 1, 1, 1, 0),
	RK3568_PLL_RATE(1600000000, 3, 200, 1, 1, 1, 0),
	RK3568_PLL_RATE(1584000000, 1, 132, 2, 1, 1, 0),
	RK3568_PLL_RATE(1560000000, 1, 130, 2, 1, 1, 0),
	RK3568_PLL_RATE(1536000000, 1, 128, 2, 1, 1, 0),
	RK3568_PLL_RATE(1512000000, 1, 126, 2, 1, 1, 0),
	RK3568_PLL_RATE(1488000000, 1, 124, 2, 1, 1, 0),
	RK3568_PLL_RATE(1464000000, 1, 122, 2, 1, 1, 0),
	RK3568_PLL_RATE(1440000000, 1, 120, 2, 1, 1, 0),
	RK3568_PLL_RATE(1416000000, 1, 118, 2, 1, 1, 0),
	RK3568_PLL_RATE(1400000000, 3, 350, 2, 1, 1, 0),
	RK3568_PLL_RATE(1392000000, 1, 116, 2, 1, 1, 0),
	RK3568_PLL_RATE(1368000000, 1, 114, 2, 1, 1, 0),
	RK3568_PLL_RATE(1344000000, 1, 112, 2, 1, 1, 0),
	RK3568_PLL_RATE(1320000000, 1, 110, 2, 1, 1, 0),
	RK3568_PLL_RATE(1296000000, 1, 108, 2, 1, 1, 0),
	RK3568_PLL_RATE(1272000000, 1, 106, 2, 1, 1, 0),
	RK3568_PLL_RATE(1248000000, 1, 104, 2, 1, 1, 0),
	RK3568_PLL_RATE(1200000000, 1, 100, 2, 1, 1, 0),
	RK3568_PLL_RATE(1188000000, 1, 99, 2, 1, 1, 0),
	RK3568_PLL_RATE(1104000000, 1, 92, 2, 1, 1, 0),
	RK3568_PLL_RATE(1100000000, 3, 275, 2, 1, 1, 0),
	RK3568_PLL_RATE(1008000000, 1, 84, 2, 1, 1, 0),
	RK3568_PLL_RATE(1000000000, 3, 250, 2, 1, 1, 0),
	RK3568_PLL_RATE(912000000, 1, 76, 2, 1, 1, 0),
	RK3568_PLL_RATE(816000000, 1, 68, 2, 1, 1, 0),
	RK3568_PLL_RATE(800000000, 3, 200, 2, 1, 1, 0),
	RK3568_PLL_RATE(700000000, 3, 350, 4, 1, 1, 0),
	RK3568_PLL_RATE(696000000, 1, 116, 4, 1, 1, 0),
	RK3568_PLL_RATE(600000000, 1, 100, 4, 1, 1, 0),
	RK3568_PLL_RATE(594000000, 1, 99, 4, 1, 1, 0),
	RK3568_PLL_RATE(500000000, 1, 125, 6, 1, 1, 0),
	RK3568_PLL_RATE(408000000, 1, 68, 2, 2, 1, 0),
	RK3568_PLL_RATE(312000000, 1, 78, 6, 1, 1, 0),
	RK3568_PLL_RATE(216000000, 1, 72, 4, 2, 1, 0),
	RK3568_PLL_RATE(200000000, 1, 100, 3, 4, 1, 0),
	RK3568_PLL_RATE(148500000, 1, 99, 4, 4, 1, 0),
	RK3568_PLL_RATE(100000000, 1, 150, 6, 6, 1, 0),
	RK3568_PLL_RATE(96000000, 1, 96, 6, 4, 1, 0),
	RK3568_PLL_RATE(74250000, 2, 99, 4, 4, 1, 0),
};

#define MHZ (1000*1000)
#define KHZ (1000)
#define MIN_FOUTVCO_FREQ ( 800 * MHZ)
#define MAX_FOUTVCO_FREQ (2000 * MHZ)

/* 计算 m&n 的最大公约数 */
rt_inline rt_uint32_t calc_gcd(rt_uint32_t m, rt_uint32_t n)
{
    rt_uint32_t t;
    while (m > 0) { if(n > m) { t = m; m = n; n = t; } m -= n; }
    return n;
}

/* 计算 postdiv1 postdiv1 foutvco */
rt_inline void calc_postdiv(rt_uint64_t fout, rt_uint32_t *postdiv1, rt_uint32_t *postdiv2, rt_uint32_t *foutvco)
{
    rt_uint64_t freq;

    if(fout < MIN_FOUTVCO_FREQ)
    {
        for(*postdiv1 = 1; *postdiv1 <= 7; (*postdiv1)++)
        {
            for(*postdiv2 = 1; *postdiv2 <= 7; (*postdiv2)++)
            {
                freq = fout * (*postdiv1) * (*postdiv2);
                if((freq >= MIN_FOUTVCO_FREQ) && (freq <= MAX_FOUTVCO_FREQ))
                {
                    *foutvco = freq;
                    return;
                }
            }
        }
        rt_kprintf("can't find postdiv1/2 to make fout %lu in 800~2000M.\n", fout);
    }
    else
    {
        *postdiv1 = 1;
        *postdiv2 = 1;
    }
}

/* 根据 PLL 配置参数计算当前频率 */
rt_inline rt_uint64_t calc_rate(rockchip_pll_cfg_t *cfg, rt_uint64_t parent_rate)
{
    rt_uint64_t rate = 0ULL;
    rt_uint32_t refdiv, fbdiv, postdiv1, postdiv2, dsmpd, frac;

    refdiv = cfg->refdiv;
    fbdiv = cfg->fbdiv;
    postdiv1 = cfg->postdiv1;
    postdiv2 = cfg->postdiv2;
    dsmpd = cfg->dsmpd;
    frac = cfg->frac;

    rate = (parent_rate*fbdiv)/(refdiv*postdiv1*postdiv2);
    if(dsmpd == 0)
    {
        rt_uint64_t frac_rate = parent_rate * (rt_uint64_t)frac;
        frac_rate /= refdiv;
        frac_rate >>= 24;
        frac_rate /= postdiv1;
        frac_rate /= postdiv2;
        rate += frac_rate;
    }

    return rate;
}

/* 计算目标频率下的最优 PLL 配置 */
rt_inline rt_uint64_t calc_best_cfg(rockchip_pll_cfg_t *cfg, rt_uint64_t parent_rate, rt_uint64_t rate)
{
    rt_uint32_t refdiv, fbdiv, postdiv1, postdiv2, dsmpd, frac;
    rt_uint32_t foutvco = rate;
    rt_uint32_t gcd = 0;

    /* 先判断预设配置中是否有参数组 */
    for(int index = 0; index < sizeof(cfgs)/sizeof(struct rockchip_pll_cfg); index++)
    {
        if(cfgs[index].rate == rate)
        {
            refdiv = cfgs[index].refdiv;
            fbdiv = cfgs[index].fbdiv;
            postdiv1 = cfgs[index].postdiv1;
            postdiv2 = cfgs[index].postdiv2;
            dsmpd = cfgs[index].dsmpd;
            frac = cfgs[index].frac;
            goto exit;
        }
    }

    /* 计算 frac postdiv1 postdiv2 refdiv fbdiv */
    frac = 0;
    calc_postdiv(rate, &postdiv1, &postdiv2, &foutvco);
    gcd = calc_gcd(parent_rate / MHZ, foutvco / MHZ);
    refdiv = parent_rate / MHZ / gcd;
    fbdiv = foutvco / MHZ / gcd;

    /* 判断输入输出频率是否需要采用小数模式进行趋近 */
    dsmpd = 1;
    if((parent_rate / MHZ * MHZ != parent_rate) || (rate / MHZ * MHZ != rate))
    {
        frac = ((foutvco % MHZ) << 24) / (parent_rate / refdiv);
        if (frac > 0) /* 判断最终是否采用小数模式 */
        {
            dsmpd = 0;
        }
    }

exit:
    cfg->refdiv = refdiv;
    cfg->fbdiv = fbdiv;
    cfg->postdiv1 = postdiv1;
    cfg->postdiv2 = postdiv2;
    cfg->dsmpd = dsmpd;
    cfg->frac = frac;

#if 0
    rt_kprintf("cfg->refdiv = %d\n", cfg->refdiv);
    rt_kprintf("cfg->fbdiv = %d\n", cfg->fbdiv);
    rt_kprintf("cfg->postdiv1 = %d\n", cfg->postdiv1);
    rt_kprintf("cfg->postdiv2 = %d\n", cfg->postdiv2);
    rt_kprintf("cfg->dsmpd = %d\n", cfg->dsmpd);
    rt_kprintf("cfg->frac = %d\n", cfg->frac);
#endif

    return calc_rate(cfg, parent_rate);
}

static void rk3568_pll_set_rate(rt_ubase_t cru, int ch, rt_uint64_t parent_rate, rt_uint64_t rate)
{
    rockchip_pll_cfg_t pll_cfg = {0};
    rt_uint32_t refdiv, fbdiv, postdiv1, postdiv2, dsmpd, frac;
    rt_uint32_t val = 0;

    if((ch < 0) || (ch > 7))
    {
        return;
    }

    if((parent_rate == 0) || (rate == 0))
    {
        return;
    }

    /* 获取最优 PLL 配置参数 */
    calc_best_cfg(&pll_cfg, parent_rate, rate);

    refdiv = pll_cfg.refdiv;
    fbdiv = pll_cfg.fbdiv;
    postdiv1 = pll_cfg.postdiv1;
    postdiv2 = pll_cfg.postdiv2;
    dsmpd = pll_cfg.dsmpd;
    frac = pll_cfg.frac;

    if(ch < 6)
    {
        /* 当接通电源或者改变 PLL 锁相环的时候必须强制锁相环进入慢速模式，以确保输出稳定时钟 */
        clrsetreg(cru + CRU_MODE, (0x03 << (ch * 2)), (CRU_PLL_MODE_SLOW << (ch * 2)));

        /* PLL 下电 */
        setreg(cru + PLL_CON(ch, 1), CRU_PLL_PD_MASK);

        /* 配置 postdiv1 fbdiv postdiv2 refdiv dsmpd frac */
        clrsetreg(cru + PLL_CON(ch, 0), (CRU_PLL_POSTDIV1_MASK | CRU_PLL_FBDIV_MASK),
            ((postdiv1 << CRU_PLL_POSTDIV1_SHIFT) | (fbdiv << CRU_PLL_FBDIV_SHIFT)));
        clrsetreg(cru + PLL_CON(ch, 1), (CRU_PLL_POSTDIV2_MASK | CRU_PLL_REFDIV_MASK),
            ((postdiv2 << CRU_PLL_POSTDIV2_SHIFT) | (refdiv << CRU_PLL_REFDIV_SHIFT)));
        clrsetreg(cru + PLL_CON(ch, 1), (CRU_PLL_DSMPD_MASK), (dsmpd << CRU_PLL_DSMPD_SHIFT));

        val = readl(cru + PLL_CON(ch, 2));
        val &= (~CRU_PLL_FRAC_MASK);
        val |= frac;
        writel(val, cru + PLL_CON(ch, 2));

        /* PLL 上电 */
        clrreg(cru + PLL_CON(ch, 1),  CRU_PLL_PD_MASK);

        /* 等待 PLL 稳定 */
        while(!(readl(cru + PLL_CON(ch, 1)) & CRU_PLL_LOCK_STATUS_MASK));

        /* PLL 进入普通模式, 输出 PLL 时钟 */
        clrsetreg(cru + CRU_MODE, (0x03 << (ch * 2)), (CRU_PLL_MODE_NORM << (ch * 2)));
    }
    else
    {
        ch -= 6;
        /* 当接通电源或者改变 PLL 锁相环的时候必须强制锁相环进入慢速模式，以确保输出稳定时钟 */
        clrsetreg(cru + PMU_CRU_MODE, (0x03 << (ch * 2)), (CRU_PLL_MODE_SLOW << (ch * 2)));

        /* PLL 下电 */
        setreg(cru + PMU_PLL_CON(ch, 1), CRU_PLL_PD_MASK);

        /* 配置 postdiv1 fbdiv postdiv2 refdiv dsmpd frac */
        clrsetreg(cru + PMU_PLL_CON(ch, 0), (CRU_PLL_POSTDIV1_MASK | CRU_PLL_FBDIV_MASK),
            ((postdiv1 << CRU_PLL_POSTDIV1_SHIFT) | (fbdiv << CRU_PLL_FBDIV_SHIFT)));
        clrsetreg(cru + PMU_PLL_CON(ch, 1), (CRU_PLL_POSTDIV2_MASK | CRU_PLL_REFDIV_MASK),
            ((postdiv2 << CRU_PLL_POSTDIV2_SHIFT) | (refdiv << CRU_PLL_REFDIV_SHIFT)));
        clrsetreg(cru + PMU_PLL_CON(ch, 1), (CRU_PLL_DSMPD_MASK), (dsmpd << CRU_PLL_DSMPD_SHIFT));

        val = readl(cru + PMU_PLL_CON(ch, 2));
        val &= (~CRU_PLL_FRAC_MASK);
        val |= frac;
        writel(val, cru + PMU_PLL_CON(ch, 2));

        /* PLL 上电 */
        clrreg(cru + PMU_PLL_CON(ch, 1),  CRU_PLL_PD_MASK);

        /* 等待 PLL 稳定 */
        while(!(readl(cru + PMU_PLL_CON(ch, 1)) & CRU_PLL_LOCK_STATUS_MASK));

        /* PLL 进入普通模式, 输出 PLL 时钟 */
        clrsetreg(cru + PMU_CRU_MODE, (0x03 << (ch * 2)), (CRU_PLL_MODE_NORM << (ch * 2)));
    }
}

static rt_uint64_t rk3568_pll_get_rate(rt_ubase_t cru, int ch, rt_uint64_t parent_rate)
{
    rt_uint32_t regs[6] = {0};
    rockchip_pll_cfg_t pll_cfg = {0};
    rt_uint32_t mode;
    rt_uint64_t rate = 0;

    if(ch < 6)
    {
        /* 读取 PLL 相关寄存器数据 */
        regs[0] = readl(cru + PLL_CON(ch, 0));
        regs[1] = readl(cru + PLL_CON(ch, 1));
        regs[2] = readl(cru + PLL_CON(ch, 2));
        regs[5] = readl(cru + CRU_MODE);
    }
    else
    {
        ch -= 6;
        /* 读取 PLL 相关寄存器数据 */
        regs[0] = readl(cru + PMU_PLL_CON(ch, 0));
        regs[1] = readl(cru + PMU_PLL_CON(ch, 1));
        regs[2] = readl(cru + PMU_PLL_CON(ch, 2));
        regs[5] = readl(cru + PMU_CRU_MODE);
    }

    /* 解析 PLL 配置 */
    pll_cfg.refdiv = (regs[1] & CRU_PLL_REFDIV_MASK) >> CRU_PLL_REFDIV_SHIFT;
    pll_cfg.fbdiv = (regs[0] & CRU_PLL_FBDIV_MASK) >> CRU_PLL_FBDIV_SHIFT;
    pll_cfg.postdiv1 = (regs[0] & CRU_PLL_POSTDIV1_MASK) >> CRU_PLL_POSTDIV1_SHIFT;
    pll_cfg.postdiv2 = (regs[1] & CRU_PLL_POSTDIV2_MASK) >> CRU_PLL_POSTDIV2_SHIFT;
    pll_cfg.dsmpd = (regs[1] & CRU_PLL_DSMPD_MASK) >> CRU_PLL_DSMPD_SHIFT;
    pll_cfg.frac = (regs[2] & CRU_PLL_FRAC_MASK) >> CRU_PLL_FRAC_SHIFT;
    mode = (regs[5] >> (ch * 2)) & 0x3;

    switch(mode)
    {
    case CRU_PLL_MODE_SLOW:    /* 低速模式 SLOW */
        rate = parent_rate;
        break;
    case CRU_PLL_MODE_NORM:       /* 普通模式 SLOW */
        rate = calc_rate(&pll_cfg, parent_rate);
        break;
    case CRU_PLL_MODE_DEEP:    /* 深度模式 SEEP */
        rate = 32768ULL;
        break;
    default:
        rate = 0ULL;
        break;
    }

    return rate;
}

static void set_enable(struct rt_clk *clk, rt_bool_t enable)
{
    /* TODO */
}

static rt_bool_t get_enable(struct rt_clk *clk)
{
    /* TODO */
    return RT_TRUE;
}

static void set_rate(struct rt_clk *clk, rt_uint64_t parent_rate, rt_uint64_t rate)
{
    struct rk3568_pll_pdata *pdata = (struct rk3568_pll_pdata *)clk->pdata;
    // rt_kprintf("cru:0x%x, ch:%d\n", pdata->cru, pdata->ch);
    rk3568_pll_set_rate(pdata->cru, pdata->ch, parent_rate, rate);
}

static rt_uint64_t get_rate(struct rt_clk *clk, rt_uint64_t parent_rate)
{
    struct rk3568_pll_pdata *pdata = (struct rk3568_pll_pdata *)clk->pdata;
    return rk3568_pll_get_rate(pdata->cru, pdata->ch, parent_rate);
}

static rt_uint64_t get_round_rate(rt_clk_t *clk, rt_uint64_t parent_rate, rt_uint64_t rate)
{
    rockchip_pll_cfg_t cfg = {0};
    return calc_best_cfg(&cfg, parent_rate, rate);
}

static struct rt_clk_ops ops =
{
    .set_enable = set_enable,
    .get_enable = get_enable,
    .set_rate = set_rate,
    .get_rate = get_rate,
    .get_round_rate = get_round_rate,
};

rt_clk_t *rt_clk_rk3568_pll_register(const char *name, const char *parent, int ch)
{
    rt_clk_t *clk = RT_NULL;
    rk3568_pll_pdata_t *pdata = RT_NULL;

    RT_ASSERT(name && parent);

    if(rt_strlen(name) == 0 || rt_strlen(parent) == 0)
    {
        clk = RT_NULL;
        goto _fail;
    }

    if((ch < 0) || (ch > 7))
    {
        clk = RT_NULL;
        goto _fail;
    }

    clk = rt_clk_create(name, &ops, 1, sizeof(rk3568_pll_pdata_t), 0);
    if(!clk)
    {
        goto _fail;
    }

    clk->parents[0] = rt_clk_find(parent);
    pdata = (rk3568_pll_pdata_t *)(clk->pdata);
#if defined(RT_USING_SMART)
    // pdata->cru = (rt_ubase_t)rt_ioremap((void *)RK3568_CRU_BASE, 0x10000);
    if(ch < 6)
    {
        pdata->cru = (rt_ubase_t)rt_ioremap((void *)CRU_BASE_ADDR, 0x10000);
    }
    else
    {
        // ch -= 6;
        pdata->cru = (rt_ubase_t)rt_ioremap((void *)PMU_CRU_BASE, 0x10000);
    }
#else
    if(ch < 6)
    {
        pdata->cru = CRU_BASE_ADDR;
    }
    else
    {
        // ch -= 6;
        pdata->cru = PMU_CRU_BASE;
    }
#endif /* RT_USING_SMART */
    pdata->ch = ch;
    // rt_kprintf("cru:0x%x, ch:%d\n", pdata->cru, pdata->ch);
    if(rt_clk_register(clk) != RT_EOK)
    {
        goto _fail;
    }

    return clk;

_fail:
    if(clk)
    {
        rt_free(clk);
    }

    return RT_NULL;
}

rt_err_t rt_clk_rk3568_pll_unregister(rt_clk_t *clk)
{
    rt_err_t ret = RT_EOK;

    ret = rt_clk_unregister(clk);
    if(!ret)
    {
        return ret;
    }

    rt_clk_delete(clk);

    return RT_EOK;
}
